﻿/*****************************************************************//**
 * \file   SurgicalRobotPlatform.cpp
 * \brief  这个文件处理微创手术机器人实验平台主控界面的代码
 *         包括主控界面的按键处理和相机图像显示
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021
 *
 * \update_Time:2024.8.15
 * \author Xyq Plx
 *
 *********************************************************************/
#include "./include/SurgicalRobotPlatform.h"
#include <QMessageBox>
#include <QDebug>
#include <QAction>
#include <QMenu>
#include <QPushButton>
#include <QWidgetAction>
#include <QFileDialog>
#include <QCloseEvent>
#include <QTextCodec>
#include "thread_test.h"

#ifdef _WIN32
#include <windows.h>
#endif

#include <QTimer>
#define _SECOND	10000000


int timer_process();

/**
 * \brief          SurgicalRobotPlatform主控界面构造函数.
 *
 * \param parent   指定组件的父组件，为空
 */
SurgicalRobotPlatform::SurgicalRobotPlatform(QWidget* parent)
	: QMainWindow(parent)

{
    ui.setupUi(this);
    //this->setStyleSheet("background-image: url(:/image/bg/bgc1.jpeg);");
    setWindowTitle(QStringLiteral("微创手术机器人实验平台  版本：202408011---V2" ));
    setWindowIcon(QIcon("hfUOT.png"));
    //robotConfigSet = new SurgicalRobotConfig();
    // 创建并设置 QTimer
    QTimer* timer = new QTimer(this);
    timer->setSingleShot(true);  // 设置为单次触发
    connect(timer, &QTimer::timeout, this, &SurgicalRobotPlatform::onTimer);
    // 启动定时器，设置为 2 秒后触发
    timer->start(2000);  // 2000 毫秒 = 2 秒
    //电机初始化
    m_obj = new ThreadObject();
    connect(ui.btVirtualDeviceStop,&QToolButton::clicked,this,&SurgicalRobotPlatform::test_Motor);
    timerUpdateMotionData.timerInit(200);
    timerUpdateMotionData.timerStart();
    connect(timerUpdateMotionData.timer,SIGNAL(timeout()),this,SLOT(UpdateRobotMotionData()));

#ifdef _WIN32
	//创建大小为1，初始化值为1的信号量
    hHapticSemaphore = CreateSemaphore(nullptr, 1, 1, nullptr);
    hHapticControlRobotSemaphore = CreateSemaphore(nullptr, 0, 1, nullptr);
    hReadRobotDataSemaphore = CreateSemaphore(nullptr, 1, 1, nullptr);
    hCutfusion = CreateSemaphore(nullptr, 0, 1, nullptr);//0,1参数值代表什么意思？？？？？
    hHapticForceFeedback = CreateSemaphore(nullptr, 0, 1, nullptr);
    hHapticreadSemaphore = CreateSemaphore(nullptr, 0, 1, nullptr);
#elif defined(__linux__)
    // 创建大小为1，初始化值为1的信号量
    sem_init(&hHapticSemaphore, 0, 1);
    sem_init(&hHapticControlRobotSemaphore, 0, 0);
    sem_init(&hReadRobotDataSemaphore, 0, 1);
    sem_init(&hCutfusion, 0, 0);
    sem_init(&hHapticForceFeedback, 0, 0);
    sem_init(&hHapticreadSemaphore, 0, 0);
#endif
    thread_pool.enqueue_pro(std::bind(&SurgicalRobotCommunication::run_code, surgicalRobotCom), 3); //机器人线程
	switch (EXPERMENTAL_VAL) {

	case EXPERMENTAL_BASE:
		//创建摄像头对象，并以线程方式运行
		surgicalCamera = new SurgicalRobotCamera();
		cameraThread.setPriority(QThread::LowestPriority);
		surgicalCamera->moveToThread(&cameraThread);

		//创建Haptic控制robot线程
		HapticControlRobotThread = new SurgicalHapticControlRobotThread();
        thread_pool.enqueue_pro(std::bind(&SurgicalHapticControlRobotThread::run_code, HapticControlRobotThread), 3); //Haptic线程

		//创建Haptic 数据处理线程
		HapticDataThread = new SurgicalHapticThread();
        thread_pool.enqueue_pro(std::bind(&SurgicalHapticThread::run_code, HapticDataThread), 1);

		//绑定信号cameraOperate和surgicalCamera的槽函数Operate
		connect(this, SIGNAL(cameraOperate(int)), surgicalCamera, SLOT(Operate(int)));
		//绑定surgicalCamera发出的信号updateImage和当前SurgicalRobotPlatform的槽函数updateImage
		//connect(surgicalCamera, SIGNAL(updateImage(QImage)), this, SLOT(updateImage(QImage)));
		connect(surgicalCamera, SIGNAL(updateImage()), this, SLOT(updateImage()));
		break;

	default:
		break;
	}


	/********************************* robot1 *********************************/
	//AUBO机械臂（IP：192.168.65.140）连接状态监控
    ui.Robot1Arm->setEnabled(false);
    SetIndicatorColor(ui.Robot1Arm, false);

    // //机械臂（IP：192.168.65.140）上电机的初始化连接状态监控
    ui.Robot1Motor1->setEnabled(true);
    SetIndicatorColor(ui.Robot1Motor1, true);
    ui.Robot1Motor2->setEnabled(true);
    SetIndicatorColor(ui.Robot1Motor2, true);
    ui.Robot1Motor3->setEnabled(true);
    SetIndicatorColor(ui.Robot1Motor3, true);
    ui.Robot1Motor4->setEnabled(true);
    SetIndicatorColor(ui.Robot1Motor4, true);
    ui.Robot1Motor5->setEnabled(true);
    SetIndicatorColor(ui.Robot1Motor5, true);

	/********************************* robot4 *********************************/
    // //franka 机械臂，连接状态监控
 //    ui.Robot4Arm->setEnabled(false);
 //    SetIndicatorColor(ui.Robot4Arm, false);
    // //franka 机械臂，对应电机的初始化连接状态监控
    // ui.Robot4Motor1->setEnabled(true);
    // SetIndicatorColor(ui.Robot4Motor1, true);
    // ui.Robot4Motor2->setEnabled(true);
    // SetIndicatorColor(ui.Robot4Motor2, true);
    // ui.Robot4Motor3->setEnabled(true);
    // SetIndicatorColor(ui.Robot4Motor3, true);
    // ui.Robot4Motor4->setEnabled(true);
    // SetIndicatorColor(ui.Robot4Motor4, true);
    // ui.Robot4Motor5->setEnabled(true);
    // SetIndicatorColor(ui.Robot4Motor5, true);

	/********************************* Haptic *********************************/
	 //主手状态监控
    ui.HapticLeft->setEnabled(false);
    SetIndicatorColor(ui.HapticLeft, false);

	/********************************* 按键状态 *********************************/
	//每个按钮的使用
    ui.btRobotStart->setEnabled(false);
    ui.btRobotRCMConfig->setEnabled(false);
    ui.btRobotMSOStep->setEnabled(false);
    ui.btVirtualDeviceStart->setEnabled(false);
    ui.btRobotShutdown->setEnabled(true);

	//各个按钮的连接函数
    connect(ui.btRobotConfig, SIGNAL(clicked()), this, SLOT(RobotConfig()));
    connect(ui.btRobotStart, SIGNAL(clicked()), this, SLOT(RobotStart()));
    connect(ui.btRobotRCMConfig, SIGNAL(clicked()), this, SLOT(RobotRCMConfig()));
    connect(ui.btRobotMSOStep, SIGNAL(clicked()), this, SLOT(RobotMSOStep()));
    connect(ui.btVirtualDeviceStart, SIGNAL(clicked()), this, SLOT(VirtualDeviceStart()));
    connect(ui.btVirtualDeviceStop, SIGNAL(clicked()), this, SLOT(VirtualDeviceStop()));
    connect(ui.btCutFusionStart, SIGNAL(clicked()), this, SLOT(CutFusionStart()));
    connect(ui.btCutFusionStart, SIGNAL(clicked()), this, SLOT(ForceThreadStart()));
    connect(ui.btCutFusionStop, SIGNAL(clicked()), this, SLOT(CutFusionStop()));
    connect(ui.btCutFusionStop, SIGNAL(clicked()), this, SLOT(ForceThreadStop()));
    connect(ui.btRobotShutdown, SIGNAL(clicked()), this, SLOT(RobotShutDown()));
    connect(ui.btOpenCamera, SIGNAL(clicked()), this, SLOT(OpenCamera()));

	//菜单按键处理
    //connect(ui.btMenu, SIGNAL(clicked()), this, SLOT(menuBarProcess()));

	/********************************* 图像显示区域属性设置*********************************/
    ui.labelCamera->setAlignment(Qt::AlignCenter);
    ui.labelCamera->setScaledContents(true);

	//启动定时器
    timerSurgicalControlRobotID = startTimer(10, Qt::PreciseTimer);
    timerSurgicalID = startTimer(1, Qt::PreciseTimer);
    timerReadRobotDataID = startTimer(10, Qt::PreciseTimer);
    timerCutfusionID = startTimer(10, Qt::PreciseTimer);
    timerTestFoceID = startTimer(1000, Qt::PreciseTimer);

	//运行线程获取设备状态信息
	//threadCommunicationFuture = QtConcurrent::run(surgicalRobotCom, &SurgicalRobotCommunication::threadGetRobotStatus);

	//初始化
    bControlStart = false;
    bVirtualFixturesOpen = false;

    connect(this,&SurgicalRobotPlatform::show_message1,this,&SurgicalRobotPlatform::show_log);

	//timer_process();
}


SurgicalRobotPlatform::~SurgicalRobotPlatform()
{
    sem_destroy(&hHapticSemaphore);
    //delete robotConfigSet;
}

void SurgicalRobotPlatform::show_log(QString str)
{
    QDateTime currentDateTime = QDateTime::currentDateTime();
    QString log = currentDateTime.toLocalTime().toString()+": " + str;

    ui.logtextEdit->append(log);
}


void SurgicalRobotPlatform::test_Motor()
{
    thread_pool.enqueue_pro(std::bind(&ThreadObject::runSomeBigWork2, m_obj), 1); //电机线程
}


void SurgicalRobotPlatform::UpdateMotorMotionData()
{
    if(functiont::instance()->StatusWord.DATA[0]!=0
        && functiont::instance()->StatusWord.DATA[1] != 0
        && functiont::instance()->StatusWord.DATA[2] != 0
        && functiont::instance()->StatusWord.DATA[3] != 0
        && functiont::instance()->StatusWord.DATA[0] != 4135
        && functiont::instance()->StatusWord.DATA[1] != 4135
        && functiont::instance()->StatusWord.DATA[2] != 4135
        && functiont::instance()->StatusWord.DATA[3] != 4135)
    {
        functiont::instance()->AxisPowerONALL();
    }
    else if(functiont::instance()->StatusWord.DATA[0] == 4135
               && functiont::instance()->StatusWord.DATA[1] == 4135
               && functiont::instance()->StatusWord.DATA[2] == 4135
               && functiont::instance()->StatusWord.DATA[3] == 4135)
    {
        Motor_status = 1; //表示电机连接状态正常，等待执行任务
    }
}

void SurgicalRobotPlatform::get_motor_status()
{
    if(Motor_status == 1)
    {
        QString ret = QString::fromStdString(functiont::instance()->ChangeJointArrayToString(functiont::instance()->theRadius, functiont::instance()->RobotAxisNum, 7));
        //请在此处编写电机运动控制代码



        motion_command(ret); //执行运动指令
    }
    else
    {
        emit show_log("当前电机状态异常");
    }
}

void SurgicalRobotPlatform::motion_command(QString command)
{
    functiont::instance()->parseStringToDoubleArray(command, functiont::instance()->tempVector);

    functiont::instance()->job = 2;
}
/*
   菜单按键处理接口，创建菜单列表.
 */
void SurgicalRobotPlatform::menuBarProcess()
{
    QMenu* pMenu = new QMenu(this);
    pMenu->setStyleSheet("color: rgb(86, 152, 214)");
    pMenu->addSeparator();

    QAction* actionSetDataSavePostion = new QAction();
    actionSetDataSavePostion->setText(QStringLiteral("保存位置"));
    pMenu->addAction(actionSetDataSavePostion);
    pMenu->addSeparator();
    pMenu->addAction(QStringLiteral("打开"));
    pMenu->addSeparator();
    pMenu->addAction(QStringLiteral("保存"));
    pMenu->addSeparator();
    pMenu->addAction(QStringLiteral("抓图"));
    pMenu->addSeparator();
    pMenu->addAction(QStringLiteral("退出 "));

    connect(actionSetDataSavePostion, SIGNAL(triggered()), this, SLOT(onActionSaveDataPostionSet()));
    ui.btMenu->setMenu(pMenu);
    ui.btMenu->showMenu();

    return;
}

/**
 * \brief   在菜单列表中点击保存数据按键时，选择保证目录位置.
 *
 */
void SurgicalRobotPlatform::onActionSaveDataPostionSet()
{
    std::string saveFilePath;
    QString savePath = QFileDialog::getExistingDirectory();

    if (!savePath.isEmpty())
    {
        saveFilePath = QDir::toNativeSeparators(savePath).toStdString();
        surgicalRobotCom->paramsPath = saveFilePath;
    }
    qDebug() << saveFilePath.c_str();

    return;
}


/**
 * \brief   根据单选按键的状态设置颜色，如果connect为true，则设置颜色为绿色；
 *          如果connect为false，则设置颜色为红色.
 *
 * \param radioButton 要是设置的单选按钮
 * \param connect     connect状态，true 或 false
 */
void SurgicalRobotPlatform::SetIndicatorColor(QRadioButton* radioButton, bool connect)
{
    if (connect) {
        radioButton->setStyleSheet("QRadioButton {border:0px;}"
            "QRadioButton::indicator {width:15px;height:15px;border-radius:7px}"
            " QRadioButton::indicator:unchecked {background-color:green;}"
        );
    }
    else {
        radioButton->setStyleSheet("QRadioButton {border:0px;}"
            "QRadioButton::indicator {width:15px;height:15px;border-radius:7px}"
            "QRadioButton::indicator:unchecked {background-color:red;}"
        );
    }
}

/**
 * \brief   微创手术平台启动后，点击机械臂配置按键时的槽函数.
 *          创建机械臂配置界面，使得器械臂启动按键可以点击
 */
void SurgicalRobotPlatform::RobotConfig()
{
    robotConfigSet = new SurgicalRobotConfig();
    robotConfigSet->setAttribute(Qt::WA_ShowModal, true);

    robotConfigSet->show();
    ui.btRobotStart->setEnabled(true);

    //emit show_message1(LOG_MES::logMessages["Info1"]);
    return;
}



/**
 * \brief  微创手术平台启动后，点击机械臂启动按键时的槽函数.
 *         完成机械臂连接登录和主手连接等初始化，使得RCM 配置按键可以点击.
 *
 */
void SurgicalRobotPlatform::RobotStart()
{
    //polyp_resectionpath.pathDataInput();   //息肉切除路径导入
    //polyp.polypDataInput();   //息肉点云导入
    //polyp_CoordinateOrigin << -0.55302, -0.2060680, -0.1543680;
    switch (EXPERMENTAL_VAL) {

    case EXPERMENTAL_BASE:
        surgicalRobotCom->initRobot(RobotArm0, sRobotConfigPara[RobotArm0].cRobotArmIP); //持电凝棒机械臂连接
        if (surgicalRobotCom->robotControl[RobotArm0]->bInitDone) {
            SetIndicatorColor(ui.Robot1Arm, true);
        }
        break;

    default:
        break;
    }
    surgicalRobotCom->initEndTool(RobotArm0);
    surgicalRobotCom->initEndTool(RobotArm1);
    surgicalRobotCom->initEndTool(RobotArm2);
    surgicalRobotCom->initEndTool(RobotArm3);

    surgicalRobotCom->HDControl[0].HapticDeviceInitial();//主手（左手）连接
    ui.btRobotRCMConfig->setEnabled(true);

    //emit surgicalRobotOperate();
    return;
}


/**
 * \brief   微创手术平台启动后，点击RCM配置按键时的槽函数.
 *          启动RCM 配置界面，设置RCM点的定位与校准.
 *
 */
void SurgicalRobotPlatform::RobotRCMConfig()
{
    rcmConfigSet = new SurgicalRobotRCM();
    rcmConfigSet->setAttribute(Qt::WA_ShowModal, true);
    rcmConfigSet->show();
    ui.btRobotMSOStep->setEnabled(true);
    return;
}


/**
 * \brief  微创手术平台启动后，点击主从操控按键的信号槽函数
 *         通知主从操控线程可以运行，主手运动转化为从手运动 .
 *         MSO(master slave operation)
 *
 */
void SurgicalRobotPlatform::RobotMSOStep()
{
    /*************************************** 获取机械臂末端位置 ***************************************/

    //m4dTransM1 << 1, 0, 0, 0,   //夹持电凝棒的夹具的厚度
    //	0, 1, 0, 0,
    //	0, 0, 1, 0.0104,
    //	0, 0, 0, 1;

    //m4dTransM2 << 1, 0, 0, 0,//绕y轴逆时针旋转90度
    //	0, 1, 0, 0,
    //	0, 0, 1, 0,
    //	0, 0, 0, 1;


    //m4dTransM3 << 1, 0, 0, 0,//0,
    //	0, 1, 0, 0,//0,
    //	0, 0, 1, 0,
    //	0, 0, 0, 1;
    //Eigen::Matrix4d m4dTooltranMR;
    //m4dTooltranM = m4dTransM1 * m4dTransM2;
    //m4dTansMatrix = m4dTooltranM * m4dTransM3;
    //m4dRobotStartposition[0] = sRobotRunData[0].sRobotDy.m4dRobotEnd;//获取初始机械臂末端位姿矩阵
    //m4dTooltranMR = m4dRobotStartposition[0] * m4dTansMatrix;//获取RCM点上端处的位姿矩阵
    //toolend_recordR.open("C:\\xiao\\experiment_data_out\\NDIRobot12.txt", std::ios::out);   //记录末端点的位置
    //for (int i = 0; i < 4; i++) {
    //	for (int j = 0; j < 4; j++) {
    //		toolend_recordR << m4dTooltranMR(i, j) << "\t";
    //	}
    //	toolend_recordR << "\n";
    //}toolend_recordR << std::endl;


    //m4dTransM2 << cos(PI / 2), 0, sin(PI / 2), 0,//绕y轴逆时针旋转90度
    //	0, 1, 0, 0,
    //	-sin(PI / 2), 0, cos(PI / 2), 0,
    //	0, 0, 0, 1;
    //m4dTransM3 << 1, 0, 0, 0,
    //	0, 1, 0, 0,
    //	0, 0, 1, 0,
    //	0, 0, 0, 1;
    //Eigen::Matrix4d m4dTooltranMT;

    ////机械臂末端到RCM点上端的旋转矩阵
    //m4dTooltranM = m4dTransM1 * m4dTransM2;
    //m4dTansMatrix = m4dTooltranM * m4dTransM3;
    //m4dRobotStartposition[0] = sRobotRunData[0].sRobotDy.m4dRobotEnd;//获取初始机械臂末端位姿矩阵
    //m4dTooltranMT = m4dRobotStartposition[0] * m4dTansMatrix;//获取RCM点上端处的位姿矩阵
    //toolend_recordT.open("C:\\xiao\\experiment_data_out\\NDITool12.txt", std::ios::out);   //记录末端点的位置
    //for (int i = 0; i < 4; i++) {
    //	for (int j = 0; j < 4; j++) {
    //		toolend_recordT << m4dTooltranMT(i, j) << "\t";
    //	}
    //	toolend_recordT << "\n";
    //}toolend_recordT << std::endl;
    /*************************************** 获取机械臂末端位置 ***************************************/





    //GVF的实验过程
    double gvf_pos[6] = { -170.195842 / 180 * M_PI, -31.313873 / 180 * M_PI, 75.017484 / 180 * M_PI, -96.092818 / 180 * M_PI, -80.562436 / 180 * M_PI, 100.503392 / 180 * M_PI };
    //fangjin实验{ -170.195842 / 180 * M_PI, -31.313873 / 180 * M_PI, 75.017484 / 180 * M_PI, -96.092818 / 180 * M_PI, -80.562436 / 180 * M_PI, 100.503392 / 180 * M_PI };
    double initPos1[6] = { -0.212966 / 180 * M_PI,1.714252 / 180 * M_PI,-117.543813 / 180 * M_PI,-69.138631 / 180 * M_PI,-93.810456 / 180 * M_PI,3.130791 / 180 * M_PI };
    double initPos2[6] = { 7.970782 / 180 * M_PI,-32.488693 / 180 * M_PI,140.845686 / 180 * M_PI,41.548710 / 180 * M_PI,-61.439334 / 180 * M_PI,90.697536 / 180 * M_PI };
    //xiaoxialin 实验
    //运动到起点位置
    double conf_pos[6] = { -165.893738 / 180 * M_PI, -15.177765 / 180 * M_PI, 115.399953 / 180 * M_PI, -27.501868 / 180 * M_PI, -79.834864 / 180 * M_PI, 83.328390 / 180 * M_PI };
    //{ -146.091954 / 180 * M_PI, -59.401158 / 180 * M_PI, 79.617125 / 180 * M_PI, 135.156357 / 180 * M_PI, -21.485335 / 180 * M_PI, -83.034582 / 180 * M_PI };



    if (bControlStart == false)
    {
        //机械臂运动到初始位置
        switch (EXPERMENTAL_VAL) {

        case EXPERMENTAL_BASE:
            if (surgicalRobotCom->robotControl[RobotArm0]) {
                surgicalRobotCom->robotControl[RobotArm0]->robotMoveJ(gvf_pos);
            }
            break;


        default:
            break;
        }



        /*************************************** 机械臂1的RCM约束 ***************************************/
        m4dTransM1 << 1, 0, 0, 0,   //夹持电凝棒的夹具的厚度
            0, 1, 0, 0,
            0, 0, 1, 0.0104,//0.0066,
            0, 0, 0, 1;

        m4dTransM2 << cos(PI / 2), 0, sin(PI / 2), 0,//绕y轴逆时针旋转90度
            0, 1, 0, 0,
            -sin(PI / 2), 0, cos(PI / 2), 0,
            0, 0, 0, 1;

        m4dTransM3 << 1, 0, 0, 0.0081,//0,
            0, 1, 0, -0.00010276033,//0,
            0, 0, 1, dInstrumentLength,
            0, 0, 0, 1;

        //机械臂末端到RCM点上端的旋转矩阵
        m4dTooltranM = m4dTransM1 * m4dTransM2;
        m4dTansMatrix = m4dTooltranM * m4dTransM3;

        m4dRobotStartposition[0] = sRobotRunData[0].sRobotDy.m4dRobotEnd;//获取初始机械臂末端位姿矩阵
        m4dTooltranM0 = m4dRobotStartposition[0] * m4dTooltranM;//获取RCM点上端处的位姿矩阵

        //设置RCM点的位置
        v3dRCMposition(0) = m4dTooltranM0(0, 3) + dRCMlength * m4dTooltranM0(0, 2);
        v3dRCMposition(1) = m4dTooltranM0(1, 3) + dRCMlength * m4dTooltranM0(1, 2);
        v3dRCMposition(2) = m4dTooltranM0(2, 3) + dRCMlength * m4dTooltranM0(2, 2);

        //设置对应机械臂的RCM点的位置
        sRobotConfigPara[0].dRCMPosition[0] = v3dRCMposition(0);
        sRobotConfigPara[0].dRCMPosition[1] = v3dRCMposition(1);
        sRobotConfigPara[0].dRCMPosition[2] = v3dRCMposition(2);
        ui.btCutFusionStart->setEnabled(true);

        ui.btVirtualDeviceStart->setEnabled(true);

        bControlStart = true;
        //cameraProc->bControlStart = true;
        bCutFusionOpen = true;

        switch (EXPERMENTAL_VAL) {

        case EXPERMENTAL_BASE:
            emit HapticControlRobotStart();
            break;

            emit HapticReadStart();

            break;

        default:
            break;
        }
        //}
    }


#if 0
    //施加引导型虚拟夹具和禁止区域型虚拟夹具的息肉切除——virtual fixture constraint
    g_distance_reord.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\g_min_distance.txt", std::ios::out);
    fr_distance_reord.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\fr_min_distance.txt", std::ios::out);

    GVF_force_record.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\g_force.txt", std::ios::out);
    FRVF_force_record.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\fr_force.txt", std::ios::out);
    toolend_record.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\toolend.txt", std::ios::out);   //记录手术器械运动轨迹
    time_experimrnt.open("C:\\feng\\experiment_result\\experiment_feng\\vf_contraint\\time_3\\time.txt", std::ios::out);
#endif



    return;
}

/**
 * \brief  微创手术平台启动后，点击运动融合按键的信号槽函数
 *         运动融合启动 .
 *
 */
void SurgicalRobotPlatform::CutFusionStart()
{
    bCutFusionOpen = true;
    emit HapticControlRobotStop();
    emit CutFusionControlStart();
    //emit ForceThreadStart();
    //experiment01Start = std::chrono::system_clock::now(); //计时器的起始时刻
    return;
}

void SurgicalRobotPlatform::CutFusionStop()
{
    bCutFusionOpen = false;

    emit CutFusionControlStop();
    //emit ForceThreadStop();

    ui.btRobotShutdown->setEnabled(true);
    //experiment01Start = std::chrono::system_clock::now(); //计时器的起始时刻

    return;
}
/**
 * \brief  微创手术平台启动后，点击虚拟夹具按键的信号槽函数
 *         虚拟夹具启动 .
 *
 */
void SurgicalRobotPlatform::VirtualDeviceStart()
{
    bVirtualFixturesOpen = true;

    emit virtualDeviceOpen();

    experiment01Start = std::chrono::system_clock::now(); //计时器的起始时刻

    return;
}






/**
 * \brief   微创手术平台启动后，点击关闭虚拟夹具按键的信号槽函数
 *          关闭虚拟夹具操作，此时可以点击关闭机械臂按键
 *
 */
void SurgicalRobotPlatform::VirtualDeviceStop() {

    bVirtualFixturesOpen = false;
    emit virtualDeviceClose();

    ui.btRobotShutdown->setEnabled(true);
}


/**
 * \brief   微创手术平台启动后，点击关闭按键的信号槽函数，在关闭虚拟夹具后可以点击关闭按键
 *          关闭机械臂等设备关闭.
 *
 */
void SurgicalRobotPlatform::RobotShutDown()
{
    bControlStart = false;

    switch (EXPERMENTAL_VAL) {

    case EXPERMENTAL_BASE:
        if (surgicalRobotCom && surgicalRobotCom->robotControl[RobotArm0]) {
            surgicalRobotCom->robotControl[RobotArm0]->robotShutdown();
        }
        break;

    default:
        break;
    }


    if (surgicalRobotCom) {
        surgicalRobotCom->HDControl[0].HapticDeviceClose();
    }

    ui.btRobotRCMConfig->setEnabled(false);
    ui.btRobotMSOStep->setEnabled(false);

    return;
}

#include <qtextcodec.h>
/**
 * \brief   打开相机按键对应的槽函数，处理打开摄像头程序.
 *          点击打开相机后，按键名称变成关闭相机，再次点击
 *          将关闭相机
 *
 */
void SurgicalRobotPlatform::OpenCamera()
{


    if (ui.btOpenCamera->text() == QStringLiteral("打开相机"))
    {
        ui.btOpenCamera->setText(QStringLiteral("关闭相机"))/*QStringLiteral("关闭相机")*/;

        switch (EXPERMENTAL_VAL) {

        case EXPERMENTAL_BASE:
            cameraThread.start();
            break;

        default:
            break;
        }
        emit cameraOperate(0);
    }
    else {
        ui.btOpenCamera->setText(QStringLiteral("打开相机"));
        ui.labelCamera->clear();

        switch (EXPERMENTAL_VAL) {

        case EXPERMENTAL_BASE:
            cameraThread.terminate();
            surgicalCamera->close();
            break;

        default:
            break;
        }
    }
    return;
}

/**
 * \brief   根据主从控制周期，当设定的周期到来后，主从操作开始执行.
 *
 * \param event
 */

void SurgicalRobotPlatform::timerEvent(QTimerEvent* event)
{

    return;
}



/**
 * \brief   是个槽函数，点击关闭按键，出发此操作.
 *
 * \param event
 */
void SurgicalRobotPlatform::closeEvent(QCloseEvent* event)
{
    //QTextCodec* codec = QTextCodec::codecForName("GB2312");
    //QTextCodec::setCodecForLocale(codec);
    //QTextCodec::setCodecForTr(QTextCodec::codecForName("gbk"));

    //QTextCodec::setCodecForCStrings(QTextCodec::codecForName("GB2312"));
    QTextCodec::setCodecForLocale(QTextCodec::codecForName("utf8"));

    QMessageBox* msgBox = new QMessageBox();

    msgBox->setStyleSheet("background-color:white");
    //msgBox->setWindowTitle(QStringLiteral("提示"));
    msgBox->setWindowTitle(QStringLiteral("提示"));
    msgBox->setWindowIcon(QIcon("hfUOT.png"));
    msgBox->setText(QStringLiteral("确定退出?"));

    QPushButton* btSure = msgBox->addButton(QStringLiteral("确定"), QMessageBox::AcceptRole);
    QPushButton* btCancel = msgBox->addButton(QStringLiteral("取消"), QMessageBox::RejectRole);
    QApplication::setQuitOnLastWindowClosed(false);

    msgBox->exec();

    if (msgBox->clickedButton() == btSure) {


        surgicalRobotCom->bAppExit = true;
        threadCommunicationFuture.waitForFinished();
        switch (EXPERMENTAL_VAL) {

        case EXPERMENTAL_BASE:
            // surgicalCamera->close();

            // cameraThread.terminate();
            // cameraThread.wait();

            // //readRobotRunDataThread.terminate();
            // //readRobotRunDataThread.wait();

            // HapticControlRobotThread->terminate();
            // HapticControlRobotThread->wait();

            // HapticDataThread->terminate();
            // HapticDataThread->wait();

            break;


        default:
            break;
        }
        event->accept();
        qApp->exit();
    }
    else {
        event->ignore();
        qDebug() << "ignore";
    }

    return;

}

/**
 * \brief   更新相机显示的槽函数.
 *
 * \param image
 */
void SurgicalRobotPlatform::updateImage(QImage image)
{
    image.scaled(ui.labelCamera->width(), ui.labelCamera->height());
    ui.labelCamera->setPixmap(QPixmap::fromImage(image));
}

/**
 * \brief   .
 *
 */
void SurgicalRobotPlatform::updateImage()
{
    qComImage.scaled(ui.labelCamera->width(), ui.labelCamera->height());

    ui.labelCamera->setPixmap(QPixmap::fromImage(qComImage));
}



void SurgicalRobotPlatform::on_btVirtualDeviceStart_clicked()
{
    std::cout << "Hello world" <<std::endl;
}


